#ifndef __VEC2D__
#define __VEC2D__

// #include <corecrt_math_defines.h>

#include <cmath>
#include <string>

#include "macro/repeat.h"
#include "macro/set_get.h"

namespace v2 {

constexpr int    __int_max__   = std::numeric_limits<int>::max();
constexpr double __float_max__ = std::numeric_limits<float>::max();
constexpr double __pi__        = 3.141592653589793238462643383279502884;
constexpr double __pi_2__      = 1.570796326794896619231321691639751442;
constexpr double __epsilon__   = 1e-10;
constexpr double __epsilon_4__ = 1e-4;
constexpr double __epsilon_5__ = 1e-5;
constexpr double __epsilon_6__ = 1e-6;

template <typename T>
std::string to_string_with_precision(const T a_value, const int n = 6) {
    std::ostringstream out;
    out.precision(n);
    out << std::fixed << a_value;
    return out.str();
}

class Vec2D {
    // clang-format off
    FOR_EACH_ARG(
        DECL_WITH_SET_GET_MODIFIER, 2, protected,
        x, double,
        y, double,
    ) // clang-format on

  public:
    Vec2D() : x_(0), y_(0) {}
    Vec2D(const double x, const double y) : x_(x), y_(y) {}
    Vec2D(const Vec2D& vec) : x_(vec.x()), y_(vec.y()) {}

    auto operator+(const Vec2D& other) -> Vec2D const { return Vec2D(x() + other.x(), y() + other.y()); }
    auto operator-(const Vec2D& other) -> Vec2D const { return Vec2D(x() - other.x(), y() - other.y()); }
    auto operator*(const Vec2D& other) -> double const { return x() * other.x() + y() * other.y(); }
    auto operator*(const double other) -> Vec2D const { return Vec2D(x() * other, y() * other); }

    template <typename _VecType>
    auto operator==(const _VecType& other) -> bool const {
        return (fabs(this->x() - other.x()) < __epsilon__) and (fabs(this->y() - other.y()) < __epsilon__);
    }
    template <typename _VecType>
    auto operator!=(const _VecType& other) -> bool const {
        return (fabs(this->x() - other.x()) > __epsilon__) or (fabs(this->y() - other.y()) > __epsilon__);
    }
    auto cross(const Vec2D& other) -> double const { return x() * other.y() - y() * other.x(); }
    auto inner(const Vec2D& other) -> double const { return x() * other.x() + y() * other.y(); }

    auto mod() -> double const { return std::hypot(x(), y()); }
    auto normailize() -> Vec2D const {
        const double l = mod();
        if (l > __epsilon__) {
            x_ref() /= l;
            y_ref() /= l;
        }
    }
    auto angle() -> double const { return std::atan2(y(), x()); }
    auto rotate(const double angle) -> Vec2D const {
        return Vec2D(x() * cos(angle) - y() * sin(angle), x() * sin(angle) + y() * cos(angle));
    }

    template <typename _VecType>
    auto distance_with(const _VecType& other) -> double const {
        return mod(*this - other);
    }

    template <typename _VecType>
    auto distance_square_with(const _VecType& other) -> double const {
        auto dis_vec = *this - other;
        return dis_vec.x * dis_vec.x + dis_vec.y * dis_vec.y;
    }

    template <typename _VecType>
    auto near(const _VecType& other, double distance) -> bool const {
        return (fabs(this->x - other.x) < distance) and (fabs(this->y - other.y) < distance);
    }
    virtual auto Str() -> std::string const { return "V(" + std::to_string(x()) + "," + std::to_string(y()) + ")"; }
};

class Point : public Vec2D {
  public:
    double       theta_;
    double&      theta_ref() { return theta_; }
    const double theta() const { return std::fmod(theta_ + 2 * __pi__, 2 * __pi__); }

    Point() : Vec2D(), theta_(0) {}
    Point(double x, double y) : Vec2D(x, y), theta_(0) {}
    Point(double x, double y, double t) : Vec2D(x, y), theta_(t) {}
    Point(const Vec2D& vec) : Vec2D(vec), theta_(0) {}
    Point(const Point& point) : Vec2D(point.x(), point.y()), theta_(point.theta()) {}

    auto Vec() -> Vec2D { return Vec2D(x(), y()); }

    virtual auto Str() -> std::string const {
        return "S(" + std::to_string(x()) + "," + std::to_string(y()) + "," + std::to_string(theta()) + ")";
    }
};

class PathPoint : public Point {
  public:
    FOR_EACH(DECL_WITH_SET_GET, 2, //
             /* curvature */
             k, double,   //
             dk, double,  //
             ddk, double, //
             /* motion */
             v, double,  // 点的速度
             a, double,  //
             da, double, //
             t, double,  // 点的时间戳，在速度预测时使用
             d, double,  // 点的运动方向 0 前进 1 后退
             /* map info */
             z, double,     // 点的高度
             ramp, double,  // 点的坡度
             left, double,  // 点的左边界
             right, double, // 点的右边界
             /* frenet info */
             s, double,   //
             l, double,   //
             dl, double,  //
             ddl, double, //
    )

    PathPoint() : Point() {}
    PathPoint(double x, double y, double theta, double c) : Point(x, y, theta), k_(c) {}
    PathPoint(const Vec2D& vec) : Point(vec) {}
    PathPoint(const Point& point) : Point(point) {}
    PathPoint(const PathPoint& pathpoint) : Point(pathpoint) {}
};

} // namespace v2

#include "macro/undefine_all.h"
#endif // __VEC2D__